Method for dynamic autocalibration of a multi-sensor tracking system and apparatus incorporating it therein

ABSTRACT

A method for dynamic auto-calibration of a multi-sensor tracking system and apparatus incorporating it therein are presented. The method and apparatus utilize information from complementary sensors, filtered by a simultaneously running filter that combines the sensor data into an estimate of the state of the monitored system to iteratively tune the bias estimate. To track a dynamic system, complimentary or redundant sensors may be combined with a model of the system so that the uncertainty in the estimated state of the dynamic system is less than the noise in the individual sensors. In addition to noise, the sensors may have bias, which has the same value whenever the system is in a particular state. While estimating the actual value of the state, the present invention allows the estimator to determine the bias. The present invention, in its most general embodiment requires complex computations. However, significant simplifications have also been developed, which reduce the necessary computational power to a manageable level. The present invention has been reduced to practice in the context of a head-tracking system employing a plurality of sensors to determine the orientation of a human head.

TECHNICAL FIELD

This invention relates to the calibration of real-time multi-sensortracking systems. More particularly, the invention relates to systemsthat use information from complimentary sensors, filtered by asimultaneously running filter component, to calibrate sensor bias and toiteratively tune the bias estimate.

BACKGROUND OF THE INVENTION

Magnetic compasses are subject to several outside influences thatproduce errors in the compass reading. One of these error sources ismagnetic variation, which is the difference between the direction of thefield lines of the earth's magnetic field and true north at anyparticular point on the earth, and which is a function of geographicposition. Another source of error is magnetic deviation, which is thedifference between magnetic north and the actual compass reading causedby local magnetic perturbations due to concentrations of metal or othermagnetically active substances. Magnetic deviation is typically strongnear large metal structures such as bridges, buildings with large steelcomponents, and ships, or near structures that transport electricitysuch as power stations and electrical cables. Furthermore, in the caseof ships or other large movable metal objects, the magnetic deviationmay vary not only with direction, but may also vary over time in aparticular direction.

Magnetic variation and static forms of magnetic deviation may becorrected in large part by modeling over the geographic region inquestion. Current approaches to bias determination rely either on amodel of the bias, or on a number of external references and an involvedcalibration procedure. In the model-based approach, a mathematical modelof how ambient magnetic fields modify compass output is used, and thecalibration procedure uses a small number of external references todetermine the parameters of the model. The heart of this method is thedetermination of a magnetic bias field that is fixed with respect to thecompass, and thus moving with respect to the earth's magnetic field. Itinvolves superimposing and subtracting the mapped magnetic bias field toleave only the earth's magnetic field in order to provide accuratecompass output. Unfortunately, this approach has the disadvantage ofbeing model-based, i.e. the bias must follow a general shape, which maynot always be applicable to the measured bias. In the external referenceapproach, a large number of external references (e.g. landmarks at knownheadings) are used to build a distortion map. In this approach, knownheading directions are used to calibrate the compass by comparing thecompass reading with a known heading and subtracting the bias for thatknown heading to generate pure heading information. This approachrequires many calibration points and a cumbersome, explicit calibrationprocedure. As a variation on this approach, prior art has also taughtthe use of a directional gyroscope to provide a second heading indicatorto be compared with the compass heading to yield bias correction. Thisvariation has the advantage of eliminating the need for predetermined,stored directions but is disadvantageous because the drift in thedirectional gyroscope causes it to become inaccurate over time. Existingsystems utilizing this approach require an explicit calibrationprocedure, and drift out of calibration when the sensor distortionchanges unless the calibration procedure is rerun. Thus, in the case ofa ship, for example, although a mapped calibration procedure may besuccessful for a given ship configuration, any change in the structureof the ship or its contents may result in a need for re-calibration.

SUMMARY OF THE PRESENT INVENTION

A method for dynamic autocalibration of a multi-sensor tracking system,having a system state x_(i) within a state space, including the stepsof: dividing the state space into a plurality of patches p_(j);providing a variable bias map including a plurality of bias entries{circumflex over (B)}_(j) with each particular one of the plurality ofbias entries {circumflex over (B)}_(j) associated with a particular oneof the plurality of patches p_(j); providing a vector z_(i) of inputsfrom a plurality of sensors for a given time step i; determining thepatch p_(j) to which the vector z_(i) from the plurality of sensorsapplies; combining the vector z_(i) with the bias entry {circumflex over(B)}_(j) corresponding to the patch p_(j) to which the input z_(i) fromthe plurality of sensors applies to provide a bias adjusted sensorinput; providing a state estimator to receive the bias adjusted sensorinput and to produce a system state estimation {circumflex over (x)}_(i)corresponding to the time step i; using a combination of the biasadjusted sensor input and the system state estimation {circumflex over(x)}_(i) to adjust the bias entry {circumflex over (B)}_(j) of thevariable bias map corresponding to the patch p_(j) to which the inputz_(i) from the plurality of sensors applies; and repeating a portion ofthe steps to provide a continual system update. In the case where thebias map includes a constant bias offset, the method further includes,the steps of: obtaining a calibration bias pair including an externallyspecified calibration bias {circumflex over (B)}* and a correspondingsystem state calibration value x *; and applying the calibration biaspair to the variable bias map to eliminate the constant bias offset. Theerror minimization utilized in the present invention may be chosen asdesired for a particular application, but is preferably performed bymeans of gradient descent utilizing a learning rate γ′ and thecombination of the bias adjusted sensor input and the system stateestimate {circumflex over (x)}_(i).

In addition to the use of a look-up table type bias map, the bias mapmay also be represented in the form of a parametric equation. An exampleof this type of bias map is provided through the use of Gaussian fuzzysets, and is represented in a preferred embodiment including at leastone gyroscope, at least one compass, and at least one tilt sensorincorporated as part of a head-mounted orientation tracker.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a flowchart presenting the steps of the present invention in ageneral embodiment, which utilizes gradient descent as the procedureused in the error minimizer;

FIG. 2 is a block diagram demonstrating the operation of the presentinvention in a general embodiment, which utilizes gradient descent asthe function used in the error minimizer;

FIG. 3 is a flowchart presenting the steps of the present invention in ageneral embodiment, which utilizes a significantly simplified gradientdescent function in the error minimizer;

FIG. 4 is a block diagram demonstrating the operation of the presentinvention in a general embodiment, which utilizes a significantlysimplified gradient descent function in the error minimizer;

FIG. 5(a) presents results from a head-tracking embodiment in the formof a chart showing the actual and estimated compass bias versus theangle for an artificial bias of five degrees, centered at a heading of100 degrees;

FIG. 5(b) provides results from a head-tracking embodiment in the formof a demonstration showing the actual and estimated compass bias versusangle for three data sets;

FIG. 6 presents results from a head-tracking embodiment in the form of atable displaying the root-mean-squared discrepancy between the sensedand estimated heading for three data sets, before and after thebias-estimation method is applied;

FIG. 7 is a flowchart presenting the steps of the present invention asutilized with Gaussian fuzzy sets to represent the bias map in ahead-tracking embodiment;

FIG. 8 is a block diagram demonstrating the operation of the presentinvention as utilized with Gaussian fuzzy sets to represent the bias mapin a head-tracking embodiment.

DETAILED DESCRIPTION

The present invention relates to a method for using information fromcomplementary sensors to calibrate sensor bias, and apparatusincorporating it therein. The following description is presented toenable one of ordinary skill in the art to make and use the inventionand to incorporate it in the context of particular applications. Variousmodifications to the preferred embodiment, as well as a variety of usesin different applications will be readily apparent to those skilled inthe art, and the general principles defined herein may be applied toother embodiments. Thus, the present invention is not intended to belimited to the embodiments shown, but is to be accorded the widest scopeconsistent with the principles and novel features disclosed herein.

In order to provide sensor bias calibration, the present inventionutilizes information from a simultaneously running filter that combinessensor data into an estimation of the state of the monitored system inorder to iteratively tune the bias estimate. To track a dynamic system,complementary, or redundant, sensors may be combined with a model of thesystem so that the uncertainty in the estimated state of the dynamicsystem is less than the noise in the individual sensors. In addition tonoise, sensors may have bias, i.e. they may produce errors that varywith the system's state but that are repeatable (having the same valuewhenever the system is in a particular state). While estimating theactual value of the state, the present invention uses an estimator todetermine the bias.

The primary application of the present invention to date is inhead-mounted orientation tracking systems, which use a compass, a tiltsensor, and gyroscopes to track movement of the head and to estimate itsorientation at any time. The particular system to which the presentinvention has been applied involves what has been termed “augmentedreality.” Specifically, the system includes an information overlay forthe natural environment, which provides visual cues regarding variouslandmarks and other environmental details. In order to provide cues in ameaningful manner, the system must track both the location of the userand the orientation of their head in terms of roll, pitch, and heading.The orientation of the user's head is tracked by inertial navigationcombined with the absolute orientation provided by a compass and tiltsensor. The compasses utilized in the trackers are known to haveorientation-specific distortions due to ambient magnetic fields. Anadvantage of utilizing the present auto-calibration method lies in itsability to allow the combined use of the gyroscopes and the compass toautomatically map out the distortion in real-time. By utilizing sensorredundancy to perform the calibration, rather than standard modelingprocedures, use of only one external landmark at a known heading isrequired. Thus, the present invention makes no “model-based” assumptionsregarding the shape of the distortion map, which is especially importantbecause distortion maps often obey no predictable pattern. Thisunpredictability is especially problematic when there is a need for ahigh degree of accuracy. Further, because the bias-determination processruns concurrently with the state estimator, changes in the distortionmap are constantly monitored and corrected. Since existing systemsrequire an explicit calibration procedure, they drift out of calibrationin response to unpredictable sensor distortion changes unless thecalibration procedure is rerun.

In the problem of estimating the unbiased state, the system herein maybe mathematically modeled as a discrete time dynamic system that issubject to perturbing noise. Furthermore, the sensors that track thestate of the system are inherently noisy. Using a combination of a modelof the system and the sensor data, an estimate of the state of thesystem at any time may be calculated. The resulting estimate providesmore accurate results than either the model or the sensors alone.Additionally, it is desirable to solve a parameter estimation problem.Specifically, the sensors have inherent biases that vary with the systemstate, but that are also repeatable. As mentioned previously, this meansthat each time the system passes through a specific state, the biasesare the same. This is in contrast to the sensor and process noise, mayvary even within the same specific state.

The system state at a particular time step i is represented by x_(i),evolves according to linear state dynamics as follows:

x _(i+1) =A _(i) x _(i) +w _(i),  (1)

where x_(i+1) represents the next system state (i.e. the system state attime step i+1), A_(i) represents a state transition matrix, and w_(i) isa noise source, where {overscore (w)}_(i)=0. Higher order non-linearterms, including driving inputs, are modeled as noise, and are alsoincluded in w_(i).

A sensor measures the system state x_(i), producing output in the formof a vector z_(i) of inputs from the plurality of sensors. Thedependence of the vector z_(i) on the system state x_(i) is given by:

z _(i) =Hx _(i) +B(x _(i))+V _(i)  (2)

where v_(i) is a noise source, where {overscore (v)}_(i)=0, H is aconstant matrix for mapping state values onto the system state x_(i),and B(x_(i)) is the state dependent sensor bias.

The estimation process is subject both to process noise (which makesopen loop modeling inaccurate) and to sensor noise and bias (which makesensing of the state inaccurate). The estimation approach combines thenext system state estimation x_(i+1) based on the system dynamics as setforth in Equation (1) above, and the vector z_(i) from the sensors asset forth in Equation (2) above, resulting in:

{circumflex over (x)} _(i+1) =A _(i) {circumflex over (x)} _(i) +K(z_(i+1) −{circumflex over (B)}(A _(i) {circumflex over (x)} _(i))−H A_(i) {circumflex over (x)} _(i))  (3)

where {circumflex over (B)}(A_(i){circumflex over (x)}_(i)) is theestimate of the sensor bias. Two elements are combined to give theupdated next state estimate, {circumflex over (x)}_(i+1) as follows: Aprior estimate from time step i, {circumflex over (x)}_(i), is appliedto the state transition matrix A_(i) to give the next system stateestimate {circumflex over (x)}_(i+1). This is modified by a correctionterm, z_(i+1)−{circumflex over (B)}(A_(i){circumflex over (x)}_(i))−HA_(i){circumflex over (x)}_(i), which represents the difference betweenwhat the sensors estimate and what the model predicts. K in the equationrepresents weight values chosen to add emphasis to either the model orthe sensors based on their relative noisiness. The remaining problem isto determine the estimated bias {circumflex over (B)}. Thisdetermination is performed by the system depicted in its generic form inFIG. 1 and 2, as discussed below.

Before system operation, the state space is divided into a plurality nmeasurement regions, or patches p₁−>p_(n), each of which has a singleassociated bias estimate. That is, when the estimated system state is inthe measurement patch p_(j), the bias estimate {circumflex over (B)}_(j)is applied. In other words, {circumflex over (B)}(A_(i){circumflex over(x)}_(i))={circumflex over (B)}_(j), if A_(i){circumflex over(x)}_(i)εp_(j). In this case, the bias estimates {circumflex over(B)}_(j) are specific to their associated patches p_(j) so they may bestored in the form of a look-up table.

Next, the quantity E[z_(i)−Hx_(i)|x_(i)|], representing the differencebetween the sensed and true values for a state, is computed.Substituting Equation (2) for the vector z_(i) to determine thisquantity, the following operation is performed:

E[z _(i) −Hx _(i) |x _(i) ]=E[B(x _(i))+v _(i) |x _(i) ]=B(x_(i))+{overscore (v)} _(i) =B(x _(i)).  (4)

This follows from the fact that the state-dependent sensor bias B(x_(i))is constant for each system state x_(i), and {overscore (v)}=0. Bycollecting statistics on this difference, the state dependent bias couldbe computed and used to adjust the sensor output. The problem would befeasible if the actual system state, x_(i), were known. However, thereis no direct access to this information and an indirect estimationmethod must be employed. Another way to cast the problem is to subtracta sensor bias estimate, then tune it so the expected value is zero. Inother words,

E[z _(i) −{circumflex over (B)}(x _(i))−Hx _(i) |x _(i)]=0,

where {circumflex over (B)}(x_(i)) is a good estimate of the sensorbias. This could be done for each patch p_(j), by searching for anestimated bias, {circumflex over (B)}_(j), where

 E[z _(i) −{circumflex over (B)} _(j) −Hx _(i) |x _(i) εp _(j)]=0.

In order to determine the bias, various error minimization algorithmsmay be utilized. One particular method, gradient descent, has been foundto be effective for the particular embodiments presented herein.Although gradient descent is presented herein to provide errorminimization, various other algorithms may be equally effectivedepending on the particular application.

Gradient descent provides a straightforward approach to this parameterestimation problem: First, construct a function of the bias estimate{circumflex over (B)}_(j), called an energy function, which iseverywhere nonnegative, and is a minimum where the bias estimate{circumflex over (B)}_(j) is optimal. Then take the gradient of thisfunction with respect to the bias estimate {circumflex over (B)}_(j) andincrementally change the bias estimate {circumflex over (B)}_(j) toreduce the value of the energy function in search of an optimum.

An example of an energy function useful for an embodiment of theinvention is provided by E_(j), which represents the statisticalvariance of d_(i) over all “visits” the system makes to particular patchp_(j): $\begin{matrix}{{E_{j} = {\frac{1}{2}\frac{1}{p_{j}}{\sum\limits_{{\hat{x}}_{i}{\varepsilon p}_{j}}\quad {{d_{i}}^{T}d_{i}}}}},} & (5)\end{matrix}$

where

 d _(i) =z _(i) −{circumflex over (B)} _(j) −H{circumflex over (x)}_(i)  (6)

where |p_(j)| is the number of “visits” the system makes to particularpatch p_(j), and the “½” is an arbitrary positive constant added forconvenience.

The gradient descent algorithm is expressed by the parameter updaterule:

Δ{circumflex over (B)} _(j)=−γ(∇_({circumflex over (B)}) _(j) E_(j))^(T)  (7)

where Δ{circumflex over (B)}_(j) is the change to the sensor biasestimate {circumflex over (B)}_(j) for a particular patch p_(j), γ is asmall positive constant controlling the learning rate, and∇_({circumflex over (B)}) _(j) E_(j) is the gradient of E_(j) withrespect to {circumflex over (B)}_(j). The gradient of Equation (5) isdetermined with respect to {circumflex over (B)}_(j) $\begin{matrix}{{{{\nabla_{{\hat{B}}_{j}}E_{j}} = {\frac{1}{p_{j}}{\sum\limits_{{\hat{x}}_{i} \in p_{j}}\quad {\left( \frac{\partial d_{i}}{\partial{\hat{B}}_{j}} \right)^{T}d_{i}}}}},{where}}{\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{H\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\prod\limits_{l = k}^{i - 1}\quad {\left( {I - {KH}} \right)A_{i}}}} \right)}K} - I}}} & (8)\end{matrix}$

where s.t. is an abbreviation for “such that”, I is an indexingvariable, I represents the identity matrix, and K, A_(i), and H are aspreviously defined relative to Equations (3), (1), and (2),respectively.

Since the energy function, Equation (8) is a sum; the incremental biaschange Δ{circumflex over (B)}_(j) can be modeled as a sequence ofchanges, one for each visit to a particular patch p_(j), i.e.${{\Delta \quad {\hat{B}}_{j}} = {\sum\limits_{{\hat{x}}_{i} \in p_{j}}\quad {\Delta \quad {\hat{B}}_{j}^{i}}}},{{\Delta \quad {\hat{B}}_{j}^{i}} = {{- \gamma^{\prime}}\quad \left( \frac{\partial d_{i}}{\partial{\hat{B}}_{j}} \right)^{T}d_{i}^{T}}},{and}$$\gamma^{\prime} = {\frac{1}{p_{j}}{\gamma.}}$

FIG. 1 provides a flow chart that summarizes the bias estimatingprocedure. As shown in the figure, it is first necessary to set alearning rate γ′ for the system, represented by box 100. Next, aninitial system state estimate {circumflex over (x)}₀ is obtained asrepresented by 102. Next, an externally specified calibration bias (x*,{circumflex over (B)}*) (e.g. a known landmark at a known heading in thecase of a head-mounted orientation tracking system) is obtained, asrepresented by 104. The remaining boxes of FIG. 1 demonstrate theroutine that is performed for each time step i during the operation ofthe system. The arrow 122 indicates repeated performance of thisroutine. Sensor sample data in the form of a vector z_(i) is firstreceived in the system from a plurality of sensors 106. After receivingthe sensor sample data vector z_(i) it is necessary to determine thecurrent measurement patch p_(j) such that x_(i−1)εp_(j), and to look upthe associated bias estimate {circumflex over (B)}_(j) from the biasestimate table as shown by 108. The bias estimate table is preferablydesigned to perform the patch p_(j) determination. As shown by box 110,the estimated system state {circumflex over (x)}_(i) is developedthrough the following equation,

{circumflex over (x)}_(i) =A _(i−1) {circumflex over (x)} _(i−1) +K(z_(i) −{circumflex over (B)} _(j) −H A _(i−1) {circumflex over (x)}_(i−1)).

The difference between the bias-adjusted sensor input and the estimatedsystem state are then calculated, as shown by box 112, by the following,

d _(i) =z _(i) −{circumflex over (B)} _(j) −H{circumflex over (x)} _(i.)

Next, as represented by box 114, the Jacobian,${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{H\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\prod\limits_{l = k}^{i - 1}\quad {\left( {I - {KH}} \right)A_{i}}}} \right)}K} - I}},$

is calculated. For summation via the Jacobian calculation of 114, thisprocedure requires the retention of past values of the state transitionmatrix A_(i) and information regarding the time step i and the patchp_(j), as represented by box 116. Next, the update, {circumflex over(B)}_(j)={circumflex over (B)}_(j)+Δ{circumflex over (B)}^(i) _(j), isapplied to the bias table where,${{\Delta \quad {\hat{B}}_{j}^{i}} = {{- \gamma^{\prime}}d_{i}^{T}\quad \frac{\partial d_{i}}{\partial{\hat{B}}_{j}}}},$

as represented by box 118. Finally, all bias estimates are periodicallyrefreshed by ∀j, {circumflex over (B)}_(j)={circumflex over(B)}_(j)−({circumflex over (B)}_(j*)−{circumflex over (B)}*), where ∀jis used as a convention for “for all j”, {circumflex over (B)}_(j*)represents the estimated calibration bias applied to the particularpatch p_(j), and {circumflex over (B)}* represents the bias provided bythe external calibration bias(x*, {circumflex over (B)}*) to subtractthe constant bias error from the entire bias map, as shown by 120.Finally, the externally specified calibration bias (x*, {circumflex over(B)}*) is obtained and applied to the bias table, as represented by 120.After the externally specified calibration bias (x*, {circumflex over(B)}*) has been applied to the bias table, the routine begins again withthe receipt of a new sensor sample data vector z_(i) 104, as representedby the arrow 122 between box 120 and 104.

FIG. 2 provides an overview of the dynamic auto-calibration estimatedbias determination method of the present invention in the form of ablock diagram. Prior to operation of the system, a learning rate γ′ isset and is represented by block 200. A bias table 202 is maintained,which is indexed by the estimated system state {circumflex over (x)}_(i)and whose output is the estimated bias {circumflex over (B)}_(j). Asingle external input that provides the bias for one state, x*, isneeded to offset the bias table in order to allow for calibration. Thus,an externally specified calibration bias (x*, {circumflex over (B)}*)204 including a sensor value reference, x*, and an externally specifiedcalibration bias {circumflex over (B)}* are provided. Furthermore, allbias estimates are periodically adjusted by ∀j, {circumflex over(B)}_(j)={circumflex over (B)}j−({circumflex over (B)}_(j*)−{circumflexover (B)}*), as represented by 206, which incorporates the externallyspecified calibration bias {circumflex over (B)}*. Biased input from aplurality of sensors 208 is aggregated in the form of a vector,z_(i)=Hx_(i)+B(x_(i))+v_(i), where x_(i) is a vector representation ofthe system state at time step i; H is a constant matrix for mappingstate values onto sensor values; B is the state dependent sensor bias;and v_(i) represents unbiased sensor noise. The estimated bias{circumflex over (B)}_(j) for a given estimated state, is subtractedfrom the biased sensor reading vector z_(i) by a first comparator 210,to provide a bias-adjusted sensor reading z_(i)−{circumflex over(B)}_(j). The bias-adjusted sensor reading z_(i)−{circumflex over(B)}_(j) is provided to an estimator 212, which then generates a systemstate estimate vector {circumflex over (x)}_(i). The estimator 212 mayalso include memory to provide for the retention of past values of thestate transition matrix A_(i) and information regarding the time step iand the patch p_(j), as discussed relative to box 116 of FIG. 1. Thesystem state estimate vector {circumflex over (x)}_(i) is thenmultiplied through the constant matrix H 214, providing a constantmatrix-multiplied estimate H{circumflex over (x)}_(i). The constantmatrix-multiplied estimate H{circumflex over (x)}_(i) and thebias-adjusted sensor reading z_(i)−{circumflex over (B)}_(j) provideinputs to a second comparator 216, with the bias-adjusted sensor inputz_(i)−{circumflex over (B)}_(j) being subtracted from the constantmatrix-multiplied estimate H{circumflex over (x)}_(i). The Jacobian ofthe resulting difference d_(i)=(z_(i)−{circumflex over(B)}_(j))−(H{circumflex over (x)}_(i)) is then calculated 218, and isthen multiplied by the learning rate −γ 200. The learning ratepreferably takes the form of a multiplicative scalar parameter betweenzero and one to provide a bias estimate adjustment figure Δ{circumflexover (B)}_(j). An incrementor 220 combines the bias estimate {circumflexover (B)}_(j) generated from the bias table 202 with the bias estimateadjustment figure Δ{circumflex over (B)}_(j) to provide an adjusted biasestimate {circumflex over (B)}_(j)+Δ{circumflex over (B)}_(j). The biastable 202 receives the adjusted bias estimate {circumflex over(B)}_(j)+Δ{circumflex over (B)}_(j) and the estimate of the system state{circumflex over (x)}_(i) in order to provide the next bias estimate{circumflex over (B)}_(j) to the first comparator 208. The change in thebias table can be computed from the difference between the estimatedstate and the corrected sensor reading. The rate of change is governedby the learning rate. Over time, the sequence of changes to the biastable drives the values it contains toward the correct values. Note thatthe incrementor 220, the learning rate −γ 200, and the Jacobiancalculation 218 are all part of what is termed the error minimizer 222of the present invention. As previously stated, there are many potentialerror minimization functions that may be employed.

The calculation of the Jacobian can be computationally intensive, aseach term in the sum contains a product of a series of matrices.However, a significant simplification may be made. Note that thesimplification is not critical to the present invention, but rather, isprovided to demonstrate a preferred method by which its operation may besimplified. The mathematical steps in the simplification will beillustrated below in order to provide a clear explanation. As describedabove, the matrix K is designed to give partial weight to sensor input,with the sensor input as previously described in Equation (3).Similarly, (I−KH) gives partial weight to the model prediction. Whenthese matrices are iteratively multiplied, they decay the terms in whichthey are contained. This is best illustrated with an example as follows:

First, rewrite Equation (8),${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{H\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\prod\limits_{l = k}^{i - 1}\quad {\left( {I - {KH}} \right)A_{i}}}} \right)}K} - I}},$

and, for simplicity set H=I and A_(i)=A. The equation then becomes$\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{I\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\left( {I - {IK}} \right)^{i - 1 - k}A^{i - 1 - k}}} \right)}K} - {I.}}$

Now set K=α·I; 0<α<1, and the equation becomes${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\left( {I - {aI}} \right)^{i - 1 - k}A^{i - 1 - k}}} \right){aI}} - I}},{or}$$\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{a\left( {\sum\limits_{{k \leq {i\quad {s.t.x_{k}}}} \in p_{j}}\quad {\left( {I - a} \right)^{i - 1 - k}A^{i - 1 - k}}} \right)} - I}$

Clearly, there exist terms in the last equation when k<<i. Since anapproximate gradient often allows gradient descent to find an optimum,i.e. by proceeding ‘gradually downhill’ rather than ‘directly downhill’,it is sufficient to ignore all the terms that include K, and approximateEquation (8) by${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {- I}},$

which is much simpler computationally. As a result, the wholebias-update algorithm may be written compactly as follows:

Δ{circumflex over (B)} _(j) ^(i)=−γ′(z _(i) −{circumflex over (B)} _(j)−H{circumflex over (x)} _(i))(−I)

or

Δ{circumflex over (B)} _(j) ^(i)=γ′(z _(i) −{circumflex over (B)} _(j)−H{circumflex over (x)} _(i)).  (9)

FIG. 3 provides a flow chart of the present invention incorporating thesimplification process just described. Note that steps 100, 102, 104,106, 108, 110, 112, and 120 are the same as were shown in FIG. 1. Themain difference in FIG. 3 lies in the 318, which represents thesimplification above and is equivalent to the Equation (9). After theupdate has been applied to the bias table, the routine begins again withthe receipt of new sensor sample data, z_(i), as represented by thearrow 322 between box 120 and 106.

FIG. 4 provides block diagram of the dynamic auto-calibration estimatedbias determination method, incorporating the simplification discussedabove. This figure is a modified version of FIG. 2, in which elements202, 204, 206, 208, 210, 212, 214, 216, 220, and 222 remain the same.However, in FIG. 4 the resulting difference, d_(i)=(z_(i)−{circumflexover (B)})−(H{circumflex over (x)}_(i)), is directly multiplied by thepositive learning rate scalar γ 400. Thus, the simplification avoids theneed to calculate the computationally complex Jacobian of the resultingdifference, d_(i)=(z_(i)−{circumflex over (B)})−(H{circumflex over(x)}_(i)). Below, an embodiment is described, which has been used totest the validity of the simplification, and to utilize the presentinvention in its preferred embodiment.

In order to test the validity of the simplification, the invention hasbeen reduced to practice in the context of a head-mounted system thattracks head orientation using a compass, tilt sensor, and three rategyroscopes. The compass is subject to unpredictable, but persistentdistortions due to local magnetic field variations. The invention wasapplied, to sensor data collected from the head tracking system. It wasalso run on artificial data generated from a simulation of a distortedcompass working in concert with gyroscopes. FIG. 5(a) presents theestimated compass bias versus the angle for an artificial bias of fivedegrees, centered at a heading of 100 degrees. The dotted linerepresents the actual bias and the solid line represents the estimatedbias. FIG. 5(b) presents the estimated compass bias versus angle forthree data sets, with the dotted line showing the bias estimatedetermined independently of the present invention. The shape of thedistortion is accurately estimated, while the overall offset remains tobe determined. To eliminate this offset in order to provide an accuratemeasure of the bias, the single external calibrating input, x*, isutilized. The externally specified calibration bias (x*,{circumflex over(B)}*) is provided for this purpose.

The implemented system uses inertial navigation to estimate the positionof the head. The goal is to estimate the angular position of the headfrom the input from the compass/tilt sensor suite and the threeorthogonally mounted gyroscopes. The compass/tilt sensor suite givesabsolute orientation information, with respect to the earth's magneticand gravitational fields. Two effects impact the compass information:magnetic declination and compass distortion. The magnetic declinationadds a constant offset to the compass output. This may be determinedeither by referring to a magnetic declination map or by reading thecompass while sighting a landmark at a known global positioning system(GPS) location, and then using the user-landmark offset to determine thetrue heading to the landmark. Compass distortion, typically only a fewdegrees, is a small but significant offset, which may vary with compassheading and with user location. If the mapping of compass heading toheading error followed a systematic and consistent pattern, e.g. simplychanged in scale or orientation from day-to-day or from site-to-site,then we would simply create a parameterized corrective map and we woulddetermine the parameters by sighting known landmarks. Such distortion isnot systematic or consistent, so creation of a parameterized correctivemap is not feasible. Thus, the autocalibration method described hereinprovides a means for finding the bias map.

For the head tracking system just described, the system state, x_(i) issix dimensional, consisting of the head's roll (r), pitch (p), andheading (h), in terms of the output of the compass/tilt sensor suite,and the rates of change of roll, pitch, and heading, as measured by therate gyroscopes. Thus, this six-dimensional vector may be representedas:

x _(i) =[r _(c) p _(c) h _(c) r _(g) p _(g) h _(g]) ^(T)

where the subscripts ‘C’ and ‘g’ refer to the compass/tilt sensor andgyroscopes, respectively. The system dynamics equation is:${x_{i + 1} = {{\left\lbrack \quad \begin{matrix}I_{3 \times 3} & {\Delta \quad t\quad A_{12i}} \\0_{3 \times 3} & I_{3 \times 3}\end{matrix} \right\rbrack x_{i}} + w_{i}}},$

where A_(12i) is the 3×3 state transition matrix of derivatives (i.e.the Jacobian of compass/tilt sensor suite values with respect togyroscopic rotations), and w_(i) is the noise term, lumping togetherprocess noise and acceleratory inputs. Because of the definition of theoutput of the particular compass/tilt sensor employed in the embodiment,and the orientations of the rate gyroscopes relative to the compass-tiltsensor, A₁₂ was determined to be,${{A_{12}(x)} = \left\lbrack \quad \begin{matrix}{{cpc}^{2}{r/a^{2}}} & {{asrcrsp}\left( {{t^{2}r} + {{2/c^{2}}p}} \right)} & {{atpc}^{2}{r/c^{2}}p} \\0 & {a/{cp}} & {- {atr}} \\0 & {{atr}/{cp}} & {{a/c^{2}}p}\end{matrix} \right\rbrack},$

where ${a = \frac{1}{\sqrt{1 + {t^{2}p} + {t^{2}r}}}},\quad {where}$

s=sin(θ), c=cos(θ), t=tan(θ), and where r, and p are the roll and pitchvalues in the state x, and Δt is the time step.

Because the state is defined in terms of the sensor outputs, H=I inEquation (2). The matrix of weights K in Equation (3) was chosen to be:$K = \left\lbrack \quad \begin{matrix}{g_{c}I_{3 \times 3}} & 0_{3 \times 3} \\0_{3 \times 3} & {g_{g}I_{3 \times 3}}\end{matrix} \right\rbrack$

where the values g_(g)=1.0, g_(c)=0.05 were determined empirically. Thisprovides the state estimation procedure, i.e. Equation (3). The biasestimation must still be specified. Because the embodiment is designedonly to detect and correct the compass heading bias, the bias estimationis as follows:

{circumflex over (B)}(x)=(0 0 {circumflex over (B)} ^(H)(x) 0 00)^(T)  (10)

The simplified version of the gradient descent algorithm provided byEquation (9) was used, and γ′ was set to 0.05. Data sets used for thetesting had on the order of 10,000 sensor inputs, collected at afrequency of 1 KHz. The testing procedure ran each data set 25 times toprovide the results shown below. Instead of a simple “look-up table” ofvalues of {circumflex over (B)}_(j), it was desired to generate afunction that smoothly interpolated the bias estimate for intermediateangles. As a result, an array of Gaussian fuzzy sets were employed whichdefined the bias estimate. The resulting general equation for the biasestimate {circumflex over (B)} is as follows: $\begin{matrix}{{\hat{B}\left( {\hat{x}}_{i} \right)} = {\sum\limits_{l = 0}^{N}\quad {W_{l}^{\frac{{{- {}}{\hat{x}}_{i}} - {c_{l}{}^{2}}}{\sigma_{W}^{2}}}}}} & (11)\end{matrix}$

where N represents the total number of Gaussian fuzzy sets in the biasmap, σ_(W) ² defines the widths of the Gaussian fuzzy sets, l is anindexing variable, and c_(l) defines the center of the lth Gaussianfuzzy set.

For purposes of the present invention, the following assignments weremade: c_(l)=10l, σ=20, N=36∥{circumflex over(x)}_(i)−c_(l)∥²=({circumflex over (x)}_(i)−10l)². The resulting biasestimate {circumflex over (B)} equation is: $\begin{matrix}{{{\hat{B}\left( {\hat{x}}_{i} \right)} = {\sum\limits_{l = 0}^{35}\quad {W_{l}e^{- \frac{{({x - {10l}})}^{2}}{\sigma_{W}^{2}}}}}},{\sigma_{W} = 20}} & (12)\end{matrix}$

thus, an array of 36 Gaussian fuzzy sets was generated, with centers 10degrees apart and widths of 20 degrees. The use of Gaussian fuzzy setsnecessitates a slight modification to the original gradient descentalgorithm. This modification will be shown in detail below.

Instead of

 Δ{circumflex over (B)}=−γ(∇_({circumflex over (B)}) E)^(T)  (13)

we have

ΔW ^(i)=−γ′(∇_(W) E)^(T)  (14)

where, ΔW^(i) is the change in W at time step i, and by the chain rule,equals $\begin{matrix}{{\Delta \quad W^{i}} = {{- \gamma^{\prime}}\quad \left( {{\nabla_{\hat{B}}E}\quad \frac{\partial\hat{B}}{\partial W}} \right)^{T}}} & (15)\end{matrix}$

or, by substituting Equation (13),${\Delta \quad W^{i}} = {\left( \frac{\partial\hat{B}}{\partial W} \right)^{T}\Delta \quad {\hat{B}}^{i}}$

Next, substituting Equation (9), $\begin{matrix}{{\Delta \quad W^{i}} = {{- \gamma^{\prime}}\quad \left( \frac{\partial\hat{B}}{\partial W} \right)^{T}\quad \left( {z_{i} - {\hat{B}}^{i} - {H{\hat{x}}_{i}}} \right)}} & (16)\end{matrix}$

Differentiating Equation (12) $\begin{matrix}{\frac{\partial\hat{B}}{\partial W_{l}} = ^{- \frac{{({x - {10l}})}^{2}}{\sigma_{W}^{2}}}} & (17)\end{matrix}$

Combining Equation (16) and Equation (17) generates the final parameterupdate formula: $\begin{matrix}{{\Delta \quad W_{l}^{i}} = {{\gamma^{\prime}\left( {z_{i} - {\hat{B}}^{i} - {H{\hat{x}}_{i}}} \right)}\quad ^{- \frac{{({\hat{x_{i}} - {10l}})}^{2}}{\sigma_{W}^{2}}}}} & (18)\end{matrix}$

The invention was first applied to artificial data with a local compassbias introduced. The data set mimicked a constant horizontal headrotation of 40 degrees per second, for 10 seconds, yielding a sequenceof 10,000 sensor data points. As previously discussed for FIG. 5(a), anartificial distortion in the compass input was introduced, which wascentered at a heading of 100 degrees, where it had a maximum of 5degrees of error. As can be seen in the solid line in the same figure,after running through the data set 25 times, the bias estimationcaptured the actual bias, albeit with a constant offset. This offset wascured by the use of an externally specified calibration bias(x*,{circumflex over (B)}*). Below, in conjunction with the descriptionof a specific embodiment of the present invention, detail regarding useof the externally specified calibration bias (x*, {circumflex over(B)}*) to subtract this constant offset from the estimated bias will beprovided.

Next, the invention was applied to three real data sets, each of whichwas collected during horizontal rotation of the head for a duration ofabout 20 seconds, and yielding about 20,000 samples of sensor input. Thedata sets were collected in the same session, and at the same physicallocation, so the compass bias was assumed to be the same. As can be seenin FIG. 5(b), the bias estimation is about the same for each data set.Further, the bias estimate is comparable to that estimated by a separateprocedure.

Another measure of the algorithm's performance is the root mean squareddiscrepancy between sensed and estimated compass heading, i.e. thestandard deviation of d_(i), defined by Equation (6):

d _(i) =z _(i) −{circumflex over (B)} _(j) −H{circumflex over (x)} _(i)

Because of sensor and process noise, this value will never decrease tozero. The table presented in FIG. 6 compares the root mean squareddiscrepancy, before and after tuning the bias estimate, for all threedata sets. The results shown in the table clearly demonstrate that thecalibration method of the present invention consistently drives thisdiscrepancy to a small value.

One means for obtaining an externally specified calibration bias (x*,{circumflex over (B)}*) in the context of the head tracking systemdescribed here is to sight a landmark at a known heading, read thecompass sensor value, and compute the true compass bias at that headingby subtracting the true heading from the compass output. The measurementpatch at the known heading may be termed the landmark measurement patch,p_(j)*, and the computed compass bias may be termed the true bias{circumflex over (B)}_(j*)={circumflex over (B)}*. Next, the normalgradient descent algorithm is applied, but all bias estimates areperiodically adjusted by

∀j,{circumflex over (B)} _(j) ={circumflex over (B)} _(j)−({circumflexover (B)} _(j*) −{circumflex over (B)}*).

This adjustment subtracts the constant bias error from the entire biasmap.

Another method of adjusting the bias map in the context of thehead-tracking embodiment involves the user during use of the system.When the head's estimated orientation is used in an augmented realitysystem, information is presented on a see-through, heads up display sothat it aligns with visible objects in the user's vicinity. Thelocations on the computer screen at which information is displayed aredetermined using estimated head orientation. Since the bias-estimationerror is constant for all head rotations, all the projected informationwill appear offset to the left or right by a constant angulardisplacement. It would be a simple matter to allow the user to control asingle scalar input while using the system, e.g. by turning a knob,until the projected information is aligned with the world view. Thus,the system may also be manually aligned.

FIG. 7 provides a flowchart of the head-tracking embodiment of thepresent invention as a modification of FIG. 1. The steps represented by100, 102, 104, 106, 110, 112, and 120 all perform the same function asthose shown in FIG. 1. The step represented by 708 provides for thecomputation of the bias estimate {circumflex over (B)} as follows:${\hat{B}}^{i} = {\sum\limits_{l = 0}^{35}\quad {W_{l}^{- \frac{{({x_{i - 1} - {10l}})}^{2}}{\sigma^{2}}}}}$

In this case, the bias is represented in the form of an equation ratherthan by a look-up table. Thus, the time step i, the patch p_(j), and thestate transition matrix A_(i), need not be saved.

The step represented by 714 provides the calculation,${\frac{\partial\hat{B}}{\partial W_{l}} = ^{- \frac{{({x - {10l}})}^{2}}{\sigma_{W}^{2}}}},\quad {\sigma_{W} = 20},$

also shown by Equation (17) above, where W is defined by Equation (13)and σ_(W)=20 for the particular embodiment. The step represented by 718provides the update for this embodiment and is equivalent to step 318 ofFIG. 3. After the update has been applied to the bias table, the routinebegins again with the receipt of sensor data vector z_(i) 106, asrepresented by the arrow 722 between box 120 and 106.

FIG. 8 provides an overview of the dynamic auto-calibration estimatedbias determination method, representing the head-tracking embodimentdiscussed above in the form of a block diagram. This figure is amodified version of FIG. 2, in which elements 200, 202, 204, 206, 208,210, 212, 214, 216, 220, and 222 are essentially unchanged. However, inFIG. 8 the resulting difference, d_(i)=(z_(i)−{circumflex over(B)})−(H{circumflex over (x)}_(i)), is multiplied by${\frac{\partial\hat{B}}{\partial W_{l}} = ^{- \frac{{({x - {10l}})}^{2}}{\sigma_{W}^{2}}}},$

as represented by 800, and as also shown by Equation (17) above, where Was defined for Equation (11) and σ_(W)=20 for the particular embodiment.The product is then multiplied by the learning rate −γ 200, and the restof the figure is analogous to FIG. 2.

Although the present invention has been discussed in the context of ahead-tracking system embodiment, there are a wide variety ofapplications to a wide range of sensors and sensor data. The presentinvention provides a means to automatically estimate sensor bias, usinginformation readily available from the concurrent sensor fusion/stateestimation process. The bias estimation process described hereinrequires no assumption about the shape of the bias curve. In otherwords, it is model free. The method is computationally tractable,avoiding storing a long history of state and sensor information, andavoiding large amounts of matrix mathematics. In the head-trackingembodiment previously discussed, the bias is estimated modulo a constantoffset, which can be determined by a single external input. Thisprovides a major advantage over existing bias-estimation techniques thatrequire a number of external inputs distributed along the range ofpossible headings.

What is claimed is:
 1. A method for dynamic autocalibration of amulti-sensor tracking system, having a system state x_(i) within a statespace, including the steps of: a. dividing the state space into aplurality of patches p_(j); b. providing a variable bias map including aplurality of bias entries {circumflex over (B)}_(j) with each particularone of the plurality of bias entries {circumflex over (B)}_(j)associated with a particular one of the plurality of patches p_(j); c.providing a vector z_(i) of inputs from a plurality of sensors for agiven time step i; d. determining the patch p_(j) to which the vectorz_(i) from the plurality of sensors applies; e. combining the vectorz_(i) with the bias entry {circumflex over (B)}_(j) corresponding to thepatch p_(j) to which the input z_(i) from the plurality of sensorsapplies to provide a bias adjusted sensor input; f. providing a stateestimator to receive the bias adjusted sensor input and to produce asystem state estimation {circumflex over (x)}_(i) corresponding to thetime step i; g. using a combination of the bias adjusted sensor inputand the system state estimation {circumflex over (x)}_(i) to adjust thebias entry {circumflex over (B)}_(j) of the variable bias mapcorresponding to the patch p_(j) to which the input z_(i) from theplurality of sensors applies; and h. repeating steps c through g foreach time step i to provide a continual system update.
 2. A method fordynamic autocalibration of a multi-sensor tracking system as set forthin claim 1, wherein the bias map includes a constant bias offset, andfurther including, before step c, the steps of: a. obtaining acalibration bias pair including an externally specified calibration bias{circumflex over (B)} and a corresponding system state calibration valuex*; and b. applying the calibration bias pair to the variable bias mapto eliminate the constant bias offset.
 3. A method for dynamicautocalibration of a multi-sensor tracking system as set forth in claim2, wherein a learning rate γ′ is determined prior to the beginning ofthe repitition of steps c through g, and wherein the adjustment of thevariable bias map is performed by means of gradient descent utilizingthe learning rate γ′ and the combination of the bias adjusted sensorinput and the system state estimate {circumflex over (x)}_(i).
 4. Amethod for dynamic autocalibration of a multi-sensor tracking system asset forth in claim 3, wherein the next state of the system x_(i+1),corresponding to the next time step i+1, is modeled by x _(i+1) =A _(i)x _(i) +w _(i), where A_(i) represents a state transition matrixcorresponding to the time step i, x_(i) represents the system statecorresponding to the time step i, and w_(i) represents a noise sourcewhere {overscore (w)}_(i)=0; and wherein the state transition matrixA_(i), the time step i, and the patch p_(j) to which it is associatedare saved and utilized during the performance of the gradient descent.5. A method for dynamic autocalibration of a multi-sensor trackingsystem as set forth in claim 3, wherein the gradient descent isperformed parametrically.
 6. A method for dynamic autocalibration of amulti-sensor tracking system as set forth in claim 4, wherein the vectorz_(i) from the plurality of sensors depends on the system state x_(i)corresponding to the time step i by the following relationship, z _(i)=Hx _(i) +B(x _(i))+v _(i) where H represents a constant matrix formapping state values onto the system state x_(i), B(x_(i)) representsstate dependent sensor bias, and v_(i) represents a noise source wherethe mean {overscore (v)}=0; wherein the state estimator providesestimates according to {circumflex over (x)} _(i+1) =A _(i) {circumflexover (x)} _(i) +K(z _(i+1) −{circumflex over (B)}(A _(i) {circumflexover (x)} _(i))−HA _(i) {circumflex over (x)} _(i)) where {circumflexover (x)}_(i+1) is estimate of the system state at time step i+1, Krepresents a matrix of noise compensation weight values, and z_(i+1)represents the vector from the plurality of sensors at time step i+1;wherein the gradient descent is performed with the calculation of theJacobian,${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{H\left( {\sum\limits_{\underset{x_{k}\varepsilon \quad p_{j}}{k \leq {i\quad {s.t.}}}}{\prod\limits_{l = k}^{i - 1}\quad {\left( {I - {KH}} \right)A_{i}}}} \right)}K} - I}},$

where l is an indexing variable, s.t. is an abbreviation for “suchthat”, where I represents the identity matrix, and whered_(i)=(z_(i)−{circumflex over (B)}_(j))−(H{circumflex over (x)}_(i));and wherein the bias entries {circumflex over (B)}_(j) are adjustedaccording to {circumflex over (B)} _(j) ={circumflex over (B)} _(j)+Δ{circumflex over (B)} _(j) ^(i), where${\Delta \quad {\hat{B}}_{j}^{i}} = {{- \gamma^{\prime}}d_{i}^{T}{\frac{\partial d_{i}}{\partial{\hat{B}}_{j}}.}}$


7. A method for dynamic autocalibration of a multi-sensor trackingsystem as set forth in claim 6, wherein the calculation of the Jacobianis simplified into the following relationship,${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {- I}},$

and is used in the adjustment of the bias entries {circumflex over(B)}_(j).
 8. A method for dynamic autocalibration of a multi-sensortracking system including the steps of: a. providing a variable bias mapparameterized by a plurality of weights; b. providing a vector z_(i) ofinputs from a plurality of sensors for a given time step i; c. combiningthe vector z_(i) with a bias estimate {circumflex over (B)} derived fromthe variable bias map to provide a bias adjusted sensor input; d.providing a state estimator to receive the bias adjusted sensor inputand to produce a system state estimation {circumflex over (x)}_(i)corresponding to the time step i; e. using a combination of the biasadjusted sensor input and the system state estimation {circumflex over(x)}_(i) to adjust the plurality of weights of the variable bias map;and f. repeating steps b through e for each time step i to provide acontinual system update.
 9. A method for dynamic autocalibration of amulti-sensor tracking system as set forth in claim 8, wherein thevariable bias map includes a constant bias offset, and furtherincluding, before step b, the steps of: a. obtaining a calibration biaspair including an externally specified calibration bias {circumflex over(B)}* and a corresponding system state calibration value x*; and b.applying the calibration bias pair to the variable bias map to eliminatethe constant bias offset.
 10. A method for dynamic autocalibration of amulti-sensor tracking system as set forth in claim 9, wherein a learningrate γ′ is determined prior to the beginning of the repitition of stepsc through g, and wherein the adjustment of the variable bias map isperformed by means of gradient descent utilizing the learning rate γ′and the combination of the bias adjusted sensor input and the systemstate estimate {circumflex over (x)}_(i).
 11. A method for dynamicautocalibration of a multi-sensor tracking system as set forth in claim10, wherein the variable bias map includes a plurality l of Gaussianfuzzy sets W_(l), and the bias estimate {circumflex over (B)} is definedby${\hat{B}\left( {\hat{x}}_{l} \right)} = {\sum\limits_{l = 1}^{N}\quad {W_{l}^{\frac{- {{{\hat{x}}_{i} - c_{l}}}^{2}}{\sigma_{W}^{2}}}}}$

where N represents the total number of Gaussian fuzzy sets W_(l) in thebias map, σ_(W) ² defines the widths of the Gaussian fuzzy sets W_(l), lis an indexing variable, and c_(l) defines the center of the lthGaussian fuzzy set W_(l).
 12. A method for dynamic autocalibration of amulti-sensor tracking system as set forth in claim 11, whereinc_(l)=10l, σ=20, N=36, and ∥{circumflex over(x)}_(i)−c_(l)∥²=({circumflex over (x)}_(i)−10l)².
 13. A method fordynamic autocalibration of a multi-sensor tracking system as set forthin claim 12 wherein the vector z_(i) of inputs from a plurality ofsensors is from a sensor system including at least one gyroscope, atleast one compass, and at least one tilt sensor.
 14. A dynamicautocalibration system for a multi-sensor tracking system as set forthin claim 13, wherein the at least one gyroscope, at least one compass,and at least one tilt sensor are utilized as part of a head-mountedorientation tracker.
 15. A dynamic autocalibration system for amulti-sensor tracking system operating in a system's state space whichis divided into a plurality of patches p_(j), and operative to receivesensor input in the form of a vector z_(i) from a plurality of sensorswhere i represents a particular time step, the dynamic autocalibrationsystem including: a. a variable bias map including a plurality of biasentries {circumflex over (B)}_(j) with each particular one of theplurality of bias entries {circumflex over (B)}_(j) associated with aparticular one of the plurality of patches p_(j), said variable bias mapoperative to receive a system state estimation {circumflex over (x)}_(i)and to determine an associated patch p_(j) and the corresponding biasentry {circumflex over (B)}_(j); b. a first comparator operative toreceive the vector z_(i) from the plurality of sensors and a bias entry{circumflex over (B)}_(j) from the bias map and to subtract the biasentry {circumflex over (B)}_(j) from the vector z₁, resulting in thedifference z_(i)−{circumflex over (B)}_(j); c. a system state estimatoroperatively connected with the first comparator to receive thedifference z_(i)−{circumflex over (B)}_(j) to provide a system stateestimation {circumflex over (x)}_(i) to the bias map; d. a constantmatrix H multiplier including a constant matrix H for mapping statevalues onto the system state x_(i), which is connected to receive thesystem state estimation {circumflex over (x)}_(i) and multiply it by theconstant matrix H, resulting in a constant matrix H multiplied systemstate estimate H{circumflex over (x)}_(i); e. a second comparatorconnected to receive the constant matrix H multiplied system stateestimate H{circumflex over (x)}_(i) from the constant matrix Hmultiplier and the difference z_(i)−{circumflex over (B)}_(j) from thefirst comparator and to subtract the constant matrix H multiplied systemstate estimate H{circumflex over (x)}_(i) from the differencez_(i)−{circumflex over (B)}_(j), resulting in d_(i)=(z_(i)−{circumflexover (B)}_(j))−(H{circumflex over (x)}_(i)); f. an error minimizerconnected to receive d_(i)=(z_(i)−{circumflex over(B)}_(j))−(H{circumflex over (x)}_(i)) from the second comparator andthe particular bias entry {circumflex over (B)}_(j) associated with theparticular patch p_(j) to which the current system state estimation{circumflex over (x)}_(i) applies and to adjust the particular biasentry {circumflex over (B)}_(j).
 16. A dynamic autocalibration systemfor a multi-sensor tracking system as set forth in claim 15, wherein thebias map includes a constant bias offset and is operative to receive acalibration bias pair including an externally including an externallyspecified calibration bias {circumflex over (B)} and a correspondingsystem state calibration value x*, and to utilize calibration bias pairto eliminate the constant bias offset.
 17. A dynamic autocalibrationsystem for a multi-sensor tracking system as set forth in claim 16wherein the error minimizer includes a learning rate γ′ and adjusts thevariable bias map by means of gradient descent, utilizing the learningrate γ′.
 18. A dynamic autocalibration system for a multi-sensortracking system as set forth in claim 17, wherein the estimator furtherincludes a memory accessible by the error minimizer, and the next stateof the system x_(i+1), corresponding to the next time step i+1, isrepresented by x _(i+1) =A _(i) x _(i) +w _(i), where A_(i) represents astate transition matrix corresponding to the time step i, x_(i)represents the system state corresponding to the time step i, and w_(i)represents a noise source where {overscore (w)}_(i)=0; and wherein thestate transition matrix A_(i), the time step i, and the patch p_(j) towhich it is associated are saved in the memory of the estimator andutilized by the error minimizer during the performance of the gradientdescent.
 19. A dynamic autocalibration system for a multi-sensortracking system as set forth in claim 17, wherein the gradient descentis performed parametrically.
 20. A dynamic autocalibration system for amulti-sensor tracking system as set forth in claim 18, wherein thevector z_(i) from the plurality of sensors depends on the system statex_(i) corresponding to the time step i by the following relationship, z_(i) =Hx _(i) +B(x _(i))+v _(i) where H represents the constant matrixfor mapping state values onto the system state x_(i), B(x_(i))represents state dependent sensor bias, and v_(i) represents a noisesource where the mean {overscore (v)}_(i)=0; wherein the estimatorprovides estimates according to {circumflex over (x)} _(i+1) =A _(i){circumflex over (x)} _(i) +K(z _(i+1) −{circumflex over (B)}(A _(i){circumflex over (x)} _(i))−H A _(i) {circumflex over (x)} _(i)) where{circumflex over (x)}_(i+1), is estimate of the system state at timestep i+1, K represents a matrix of noise compensation weight values, andz_(i+1), represents the vector from the plurality of sensors at timestep i+1; wherein the error minimizer performs the gradient descent withthe calculation of the Jacobian,${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {{{H\left( {\sum\limits_{\underset{x_{k}\varepsilon \quad p_{j}}{k \leq {i\quad {s.t.}}}}{\prod\limits_{l = k}^{i - 1}\quad {\left( {I - {KH}} \right)A_{i}}}} \right)}K} - I}},$

where l is an indexing variable, s.t. is an abbreviation for “suchthat”, and where I represents the identity matrix; and wherein the biasentries {circumflex over (B)}_(j) of the bias table are adjustedaccording to {circumflex over (B)} _(j) ={circumflex over (B)} _(j)+Δ{circumflex over (B)} _(j) ^(i), where${\Delta \quad {\hat{B}}_{j}^{i}} = {{- \gamma^{\prime}}d_{i}^{T}{\frac{\partial d_{i}}{\partial{\hat{B}}_{j}}.}}$


21. A dynamic autocalibration system for a multi-sensor tracking systemas set forth in claim 18, wherein the calculation of the Jacobian in theerror minimizer is simplified into the following relationship,${\frac{\partial d_{i}}{\partial{\hat{B}}_{j}} = {- I}},$

and is used in the adjustment of the bias entries {circumflex over(B)}_(j).
 22. A dynamic autocalibration system for a multi-sensortracking system operating in a system's state space, and operative toreceive sensor input in the form of a vector z_(i) from a plurality ofsensors where i represents a particular time step, the dynamicautocalibration system including: a. a variable bias map parameterizedby a plurality of weights; b. a first comparator operative to receivethe vector z_(i) from the plurality of sensors and a bias entry{circumflex over (B)}_(i) for the time step i from the bias map and tosubtract the bias entry {circumflex over (B)}_(i) from the vector z_(i),resulting in the difference z_(i)−{circumflex over (B)}_(i); c. a systemstate estimator operatively connected first comparator to receive thedifference z_(i)−{circumflex over (B)}_(i) to provide a system stateestimation {circumflex over (x)}_(i) to the bias map; d. a constantmatrix H multiplier including a constant matrix H for mapping statevalues onto the system state x_(i), which is connected to receive thesystem state estimation {circumflex over (x)}_(i) and multiply it by theconstant matrix H, resulting in a constant matrix H multiplied systemstate estimate H{circumflex over (x)}_(i); e. a second comparatorconnected to receive the constant matrix H multiplied system stateestimate H{circumflex over (x)}_(i) from the constant matrix Hmultiplier and the difference z_(i)−{circumflex over (B)}_(i) from thefirst comparator and to subtract the constant matrix H multiplied systemstate estimate H{circumflex over (x)}_(i) from the differencez_(i)−{circumflex over (B)}_(i), resulting in d_(i)=(z_(i)−{circumflexover (B)}_(i))−(H{circumflex over (x)}_(i)); f. an error minimizerconnected to receive d_(i)=(z_(i)−{circumflex over(B)}_(i))−(H{circumflex over (x)}_(i)) from the second comparator andthe particular bias entry {circumflex over (B)}_(i) and to adjust theparticular bias entry {circumflex over (B)}_(j).
 23. A dynamicautocalibration system for a multi-sensor tracking system as set forthin claim 22, wherein the bias map includes a constant bias offset and isoperative to receive a calibration bias pair including an externallyincluding an externally specified calibration bias {circumflex over(B)}* and a corresponding system state calibration value x*, and toutilize the calibration bias pair eliminate the constant bias offset.24. A dynamic autocalibration system for a multi-sensor tracking systemas set forth in claim 23 wherein the error minimizer includes a learningrate γ′ and adjusts the variable bias map by means of gradient descent,utilizing the learning rate γ′.
 25. A dynamic autocalibration system fora multi-sensor tracking system as set forth in claim 24, wherein thevariable bias map includes a plurality l of Gaussian fuzzy sets, and thebias estimate {circumflex over (B)} is defined by${\hat{B}\left( {\hat{x}}_{i} \right)} = {\sum\limits_{l = 0}^{N}\quad {W_{l}^{\frac{{{- {}}{\hat{x}}_{i}} - {c_{l}{}^{2}}}{\sigma_{W}^{2}}}}}$

where N represents the total number of Gaussian fuzzy sets W_(l) in thebias map, σ_(W) ² defines the widths of the Gaussian fuzzy sets W_(l), lis an indexing variable, and c_(l) defines the center of the lthGaussian fuzzy set W_(l).
 26. A dynamic autocalibration system for amulti-sensor tracking system as set forth in claim 25, whereinc_(l)=10l, σ=20, N=36, and ∥{circumflex over(x)}_(i)−c_(l)∥²=({circumflex over (x)}_(i)−10l)².
 27. A dynamicautocalibration system for a multi-sensor tracking system as set forthin claim 26 wherein the vector z_(i) of inputs from a plurality ofsensors is from a sensor system including at least one gyroscope, atleast one compass, and at least one tilt sensor.
 28. A dynamicautocalibration system for a multi-sensor tracking system as set forthin claim 27, wherein the at least one gyroscope, at least one compass,and at least one tilt sensor are utilized as part of a head-mountedorientation tracker.